#include "guass_newton.h"
#include "ku.h"

using namespace std;
using namespace Eigen;

// 代价函数的计算模型
void guass_newtown_3d(const vector<double> &x_data, const vector<double> &y_data, int *number,double *a_1,double *b_1,double *c_1){
  double a=0.00001;
  double k=0.1;
  double c=17;
  int N = *number;
  int iterations = 100;    // 迭代次数
  double cost = 0, lastCost = 0;  // 本次迭代的cost和上一次迭代的cost
  chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
  for (int iter = 0; iter < iterations; iter++) {
    Matrix3d H = 0.000000000000001*Matrix3d::Identity();             // Hessian = J^T W^{-1} J in Gauss-Newton
    Vector3d b = Vector3d::Zero();             // bias
    cost = 0;
    int first = (int)initime(91107)-(int)initime(90808);
    for (int i = first; i < N; i++) {
      double xi = x_data[i], yi = y_data[i];  // 第i个数据点
      double error = yi - funcy(a,k,c,xi);
      // std::cout<<"err:"<<error<< std::endl;
      double eps = 1e-9;
      Eigen::Vector3d J; // 雅可比矩阵
      J[0] = -(funcy(a+eps,k,c,xi)-funcy(a,k,c,xi))/eps;
      J[1] = -(funcy(a,k+eps,c,xi)-funcy(a,k,c,xi))/eps;
      J[2] = -(funcy(a,k,c+eps,xi)-funcy(a,k,c,xi))/eps;
      H +=  J * J.transpose();
      b -=  error * J;
      cost += error * error;
      
    }
    //std::cout<<"H:"<<H<<"\n b:"<<b<< std::endl;
    // //求解线性方程 Hx=b
    Vector3d dx;
    dx = H.ldlt().solve(b);
    // std::cout<<"dx:"<<dx;
    if (isnan(dx[0])) {
      cout << "result is nan!" << endl;
      break;
    }
  a  += dx[0];
  k  += dx[1];
  c  += dx[2];
  lastCost = cost;
  }
  *a_1 = a;
  *b_1 = k;
  *c_1 = c;
  chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
  chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1);
  std::cout << "solve time cost = " << time_used.count() << " seconds. " << endl;
  std::cout << "estimated a b c = " << a << ", " << k << ", " << c << endl;
}

void guass_newtown_2d(const vector<double> &x_data, const vector<double> &y_data, int *number,double *a_2,double *c_2){
  double a = 0.1;
  double c = 120;
  int N = *number;
  int iterations = 30;    // 迭代次数
  double cost = 0, lastCost = 0;  // 本次迭代的cost和上一次迭代的cost
  double t_2 = initime(91206)-initime(90500);
  chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
  for (int iter = 0; iter < iterations; iter++) {
    Matrix2d H = 0.01*Matrix2d::Identity();             // Hessian = J^T W^{-1} J in Gauss-Newton
    Vector2d b = Vector2d::Zero();             // bias
    cost = 0;
    for (int i = 0; i < N; i++) {
      double xi = x_data[i], yi = y_data[i];  // 第i个数据点
      double error = yi - funcx(a,c,xi,t_2);
      double eps = 1e-9;
      Vector2d J; // 雅可比矩阵
      J[0] = -(funcx(a+eps,c,xi,t_2)-funcx(a,c,xi,t_2))/eps;
      J[1] = -(funcx(a,c+eps,xi,t_2)-funcx(a,c,xi,t_2))/eps;
      H +=  J * J.transpose();
      b -=  error * J;
      cost += error * error;
    }
    //std::cout<<"H:"<<H<<"\n b:"<< b <<std::endl;
    // 求解线性方程 Hx=b
    Vector2d dx = H.ldlt().solve(b);
    //std::cout<<"dx:"<<dx;
  if (isnan(dx[0])) {
      cout << "result is nan!" << endl;
      break;
  }
  a += dx[0];
  c += dx[1];
  lastCost = cost;
  }
  *a_2 = a;
  *c_2 = c;
  chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
  chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1);
  std::cout << "solve time cost = " << time_used.count() << " seconds. " << endl;
  std::cout << "estimated a c = " << a << ", " << c << endl;
}

void variance(vector<double>&x_data,vector<double>&y_data,int *number){
  double V=0;
  int num = *number;
  for(int i=0;i<num;i++){
    double xi = x_data[i], yi = y_data[i]; 
    V = V+ (yi-xi)*(yi-xi);
  }
  V = V/(num-1);
  std::cout<<"variance="<<V<<endl;
}